#include <QDebug>
#include <QFile>
#include <QDomDocument>
#include <QDomNodeList>
#include <QDomElement>
#include <QStringList>

#include "world.h"
#include "vehicle.h"
#include "driver.h"
#include "streetmap.h"
#include "utilities.h"
#include "trafficcontrol.h"

using namespace Utilities;

// Singleton instance
World* World::gInstance = 0;

// Length of ticks in simulated world
qreal World::gSimTimeStep = 0.1f; // in seconds

QTime World::gSimTime;

World::World()
{
  gSimTime.setHMS( 0, 0, 0, 0 ) ;
  mExiting = false;
  mStreetMap = 0;
  mTotalTicks = 0;
  //loadMapXML("config/test_map.xml");
  //mStreetMap = new StreetMap("config/test_map.xml", QSize(500,500));
  //readScenarioXML("config/test_scenario.xml");
  qDebug( "WORLD: HOI!" );
}

World::~World()
{
  mExiting = true;
  clearScenario();
  qDebug( "WORLD: BOI!" );
}

World* World::instance()
{
  if (!gInstance)
  {
    gInstance = new World;
    if (!gInstance)
      qDebug("WORLD: ERROR - World instance not created!");
  }
  return gInstance;
}

bool World::destroyInstance()
{
  if (gInstance)
  {
    delete gInstance;
    gInstance = 0;
    return true;
  }
  return false;
}

int World::loadMapXML(const QString &fileName)
{
  clearMap();
  mStreetMap = new StreetMap(fileName, QSize(500,500));
  mMapFilename = fileName;
  qDebug() << "WORLD: loadMapXML>" << fileName;
  return 1;
}

void World::clearMap()
{
  if (mStreetMap)
  {
    delete mStreetMap;
    qDebug() << "WORLD: Clearing Map";
    mMapFilename = "";
  }
  mStreetMap = 0;
}

void World::clearScenario()
{
  gSimTime.setHMS(0,0,0);
  foreach(Driver *d, mDrivers) delete d;
  mDrivers.clear();
  foreach(Vehicle *v, mVehicles) delete v;
  mVehicles.clear();
  foreach(DriverType *dt, mDriverTypes) delete dt;
  mDriverTypes.clear();
  foreach(VehicleType *vt, mVehicleTypes) delete vt;
  mVehicleTypes.clear();
  foreach(TrafficControl *tc, mTrafficControls) delete tc;
  mTrafficControls.clear();

  qDebug( "WORLD: Scenario cleared!" );
}


int World::loadTypesXML(const QString &fileName)
{
  qDebug() << "XML: Reading type XML file>" << fileName;

  QDomDocument doc( "Types" );
  QFile file( fileName );
  if( !file.open( QIODevice::ReadOnly ) )
  {
    qDebug() << "XML: ERROR could not open type file>" << fileName;
    return -1;
  }
  if( !doc.setContent( &file  ) )
  {
    qDebug() << "XML: ERROR could not read type XML (malformed?)>" << fileName;
    file.close();
    return -2;
  }
  file.close();

  // Clear old lists, if we have vehicles/driver retain pointers to their types,
  // this may be dangerous. We just copy the type data into the instance data,
  // for now, so it should be ok.
  mVehicleTypes.clear();
  mDriverTypes.clear();

  // Read vehicles types
  QDomNodeList vTypes = doc.elementsByTagName("VehicleType");
  for (int n1=0; n1<vTypes.size(); ++n1)
  {
    QDomElement elem1 = vTypes.at(n1).toElement();
    VehicleType *vt = new VehicleType();
    vt->name = elem1.attribute("name");

    qDebug() << "XML: read VehicleType>" << vt->name;

    QDomNodeList children = elem1.childNodes();
    for (int i=0; i<children.size(); ++i)
    {
      QDomElement elem2 = children.at(i).toElement();
      if (elem2.tagName() == "Graphic")
        vt->graphic = elem2.text();
      else if (elem2.tagName() == "WheelBase")
        vt->wheelBase = (elem2.text().toFloat());
      else if (elem2.tagName() == "Mass")
        vt->mass = (elem2.text().toFloat());
      else if (elem2.tagName() == "DragConstant")
        vt->dragConstant = (elem2.text().toFloat());
      else if (elem2.tagName() == "MaxEngineForce")
        vt->maxEngineForce = (elem2.text().toFloat());
    }
    mVehicleTypes.push_back(vt);
  }

  // Read driver types
  QDomNodeList dTypes = doc.elementsByTagName("DriverType");
  for (int n2=0; n2<dTypes.size(); ++n2)
  {
    QDomElement elem1 = dTypes.at(n2).toElement();

    DriverType *dt = new DriverType();
    dt->name = elem1.attribute("name");

    qDebug() << "XML: read DriverType>" << dt->name;

    QDomNodeList children = elem1.childNodes();
    for (int i=0; i<children.size(); ++i)
    {
      QDomElement elem2 = children.at(i).toElement();
      if (elem2.tagName() == "FollowDistance")
        dt->followDistance = elem2.text().toFloat();
      else if (elem2.tagName() == "VisibilityDistance")
        dt->visibilityDistance = elem2.text().toFloat();
      else if (elem2.tagName() == "ReactionTime")
        dt->reactionTime = elem2.text().toFloat();
      else if (elem2.tagName() == "PreferredTurnSpeed")
        dt->preferredTurnSpeed = elem2.text().toFloat();
      else if (elem2.tagName() == "Jerk")
        dt->jerk = elem2.text().toFloat();
      else if (elem2.tagName() == "DefaultSpeed")
        dt->defaultSpeed = elem2.text().toFloat();
    }
    mDriverTypes.push_back(dt);
  }

  return 1;
}

int World::loadScenarioXML(const QString &fileName)
{
  gSimTime.setHMS(0,0,0);
  clearScenario();

  qDebug() << "XML: Reading scenario XML file>" << fileName;

  QDomDocument doc( "Scenario" );
  QFile file( fileName );
  if( !file.open( QIODevice::ReadOnly ) )
  {
    qDebug() << "XML: ERROR could not open scenario file>" << fileName;
    return -1;
  }

  if( !doc.setContent( &file  ) )
  {
    qDebug() << "XML: ERROR could not read XML (malformed?)>" << fileName;
    file.close();
    return -2;
  }
  file.close();

  // Read map file
  QDomNodeList mapTagList = doc.elementsByTagName("Map");
  if (mapTagList.isEmpty())
    return -7;
  else
  {
    QDomElement elem1 = mapTagList.at(0).toElement();
    if (elem1.text() == mMapFilename)
      qDebug() << "XML: Map already loaded:" << fileName;
    else
      loadMapXML(elem1.text());
  }

  // Read types file
  QDomNodeList typesTagList = doc.elementsByTagName("Types");
  if (typesTagList.isEmpty())
    return -8;
  else
  {
    QDomElement elem1 = typesTagList.at(0).toElement();
    loadTypesXML(elem1.text());
  }

  // Read vehicles instances
  QDomNodeList vehs = doc.elementsByTagName("Vehicle");
  for (int n3=0; n3<vehs.size(); ++n3)
  {
    QDomElement elem1 = vehs.at(n3).toElement();
    QString vName = elem1.attribute("name");
    QString vtName = elem1.attribute("type");

    qDebug() << "XML: read Vehicle>" << vName;

    Vehicle *newVeh = addVehicle(vName, vtName);
    if (!newVeh)
      return -3;

    // Vehicle Attributes
    QDomNodeList children = elem1.childNodes();
    for (int i=0; i<children.size(); ++i)
    {
      QDomElement elem2 = children.at(i).toElement();
      if (elem2.tagName() == "Location")
      {
        QString valStr = elem2.text();
        qreal x = valStr.section(" ", 0, 0).toFloat();
        qreal y = valStr.section(" ", 1, 1).toFloat();
        newVeh->setPosition(Point(x,y));
      }
      else if (elem2.tagName() == "Direction")
      {
        newVeh->setDirection(elem2.text().toFloat());
      }
    }

    // Read driver instance
    QDomNodeList drvs = elem1.elementsByTagName("Driver");
    for (int n4=0; n4<drvs.size(); ++n4)
    {
      QDomElement elem2 = drvs.at(n4).toElement();
      QString dName = elem2.attribute("name");
      QString dtName = elem2.attribute("type");
      //QString vName = elem2.attribute("vehicle");

      qDebug() << "XML: read Driver>" << dName;

      Driver *newDrvr = addDriver(dName, dtName);
      if (!newDrvr)
        return -4;
      // Get in vehicle
      newDrvr->getIn(newVeh);

      Point destination;
      QDomNodeList children = elem2.childNodes();
      for (int i=0; i<children.size(); ++i)
      {
        QDomElement elem3 = children.at(i).toElement();
//        if (elem3.tagName() == "Destination")
//        {
//          QString valStr = elem3.text();
//          qreal x = valStr.section(" ", 0, 0).toFloat();
//          qreal y = valStr.section(" ", 1, 1).toFloat();
//          destination.setX(x);
//          destination.setY(y);
//          newDrvr->setDestination(destination);
//        }
        if (elem3.tagName() == "Path")
        {
          QDomNodeList children2 = elem3.childNodes();
          for (int j=0; j<children2.size(); ++j)
          {
            QDomElement elem4= children2.at(j).toElement();
            if (elem4.tagName().compare("Intersections",Qt::CaseInsensitive) == 0)
            {
              QString valStr = elem4.text();
              QStringList strList = valStr.split(" ", QString::SkipEmptyParts);
              for (int si=0; si<strList.size(); si++)
                newDrvr->addWaypoint(strList.at(si));
              //destination = strList.last();
            }
            else // waypoints
            {
              QString valStr = elem4.text();
              qreal x = valStr.section(" ", 0, 0).toFloat();
              qreal y = valStr.section(" ", 1, 1).toFloat();
              newDrvr->addWaypoint(Point(x,y));
            }
          }
        }
      }

    }
  }
  mScenarioFilename = fileName;
  return 1;
}

int World::loadTrafficControlXML(const QString &fileName)
{
  qDebug() << "XML: Reading traffic control XML file>" << fileName;

  QDomDocument doc( "TrafficControls" );
  QFile file( fileName );
  if( !file.open( QIODevice::ReadOnly ) )
  {
    qDebug() << "XML: ERROR could not open traffic control file>" << fileName;
    return -1;
  }

  if( !doc.setContent(&file) )
  {
    qDebug() << "XML: ERROR could not read traffic control XML (malformed?)>" << fileName;
    file.close();
    return -2;
  }
  file.close();
  mTrafficControls.clear();
  mTrafficControlFilename = fileName;

  // Read traffic control file
  QDomNodeList mapTagList = doc.elementsByTagName("Map");
  if (mapTagList.isEmpty())
    return -7;
  else
  {
    QDomElement elem1 = mapTagList.at(0).toElement();
    if (elem1.text() != mMapFilename)
    {
      qDebug() << "XML: ERROR control file is not intended for currently loaded map! It is intended for" << elem1.text();
      return -3;
    }
  }

  // Read traffic control file
  QDomNodeList controlTags = doc.elementsByTagName("Control");
  for (int i=0; i<controlTags.size(); i++)
  {
    // Control tag
    TrafficControl *tc = new TrafficControl();
    QDomElement controlTag = controlTags.at(i).toElement();
    const StreetMap::Intersection *isec = NULL;

    // Control type
    if (controlTag.attribute("type").compare("LIGHT",Qt::CaseInsensitive)==0)
      tc->setType(TrafficControl::TYPE_LIGHT);
    else if (controlTag.attribute("type").compare("STOPSIGN",Qt::CaseInsensitive)==0)
      tc->setType(TrafficControl::TYPE_STOPSIGN);
    else
      tc->setType(TrafficControl::TYPE_NONE);

    // Read Control tag children
    QDomNodeList controlChildren = controlTag.childNodes();
    for (int j=0; j<controlChildren.size(); j++)
    {
      QDomElement controlChildTag = controlChildren.at(j).toElement();


      if (controlChildTag.tagName().compare("INTERSECTION",Qt::CaseInsensitive)==0)
      {
        // Intersection Tag
        isec = mStreetMap->getIntersection(controlChildTag.text());
        tc->setIntersection(isec);
      }
      else if (controlChildTag.tagName().compare("STATE",Qt::CaseInsensitive)==0)
      {
        // State tag
        QDomElement stateTag = controlChildTag.toElement();
        TrafficControl::State *state = new TrafficControl::State(stateTag.attribute("name"));

        QDomNodeList stateChildren = stateTag.childNodes();
        for (int k=0; k<stateChildren.size(); k++)
        {
          QDomElement stateChildTag = stateChildren.at(k).toElement();

          if (stateChildTag.tagName().compare("GREENTIME",Qt::CaseInsensitive)==0)
            state->setGreenTime(stateChildTag.text().toFloat());
          else if (stateChildTag.tagName().compare("YELLOWTIME",Qt::CaseInsensitive)==0)
            state->setYellowTime(stateChildTag.text().toFloat());
          else if (stateChildTag.tagName().compare("POSTREDTIME",Qt::CaseInsensitive)==0)
            state->setPostRedTime(stateChildTag.text().toFloat());
          else if (stateChildTag.tagName().compare("CONNECTION",Qt::CaseInsensitive)==0)
          {
            // Connection tag
            QStringList strList = stateChildTag.text().split(",", QString::SkipEmptyParts);

            const StreetMap::Road *from = mStreetMap->getRoad(strList.at(0));
            unsigned int fromLane = strList.at(1).toInt();
            const StreetMap::Road *to = mStreetMap->getRoad(strList.at(2));
            unsigned int toLane = strList.at(3).toInt();

            TrafficControl::Connection *conn = new TrafficControl::Connection(from, fromLane, to, toLane);
            state->addConnection(conn);
          }
        }
        // Add state to control
        tc->addState(state);
      }
    }
    // Add traffic control to world
    if (isec)
      mTrafficControls[isec] = tc;
  }

  return 0;
}

void World::reloadScenario()
{
  clearScenario();
  if (!mScenarioFilename.isEmpty())
    loadScenarioXML(mScenarioFilename);
  if (!mTrafficControlFilename.isEmpty())
    loadTrafficControlXML(mTrafficControlFilename);

}

void World::setExiting(bool exiting)
{
  mExiting=exiting;
}

bool World::exiting()
{
  return mExiting;
}

void World::addVehicle(Vehicle *v)
{
  mVehicles.push_back(v);
  //qDebug() << "WORLD: addVehicle> Adding vehicle" << v->name();
}

Vehicle* World::addVehicle(const QString &name, const QString &typeName)
{
  // Find a name for this vehicle that doesn't already exist.
  int vCount = mVehicles.size();
  QString origName = name;
  QString actualName = origName;
  if (name.isEmpty())
  {
    origName = "Vehicle";
    actualName = origName+QString::number(vCount);
  }
  Vehicle *exists = findVehicle(actualName);
  int n = vCount+1;
  while (exists != NULL)
  {
    exists = findVehicle(origName+QString::number(n));
    n++;
  }

  // Find vehicle type
  int index = -1;
  for (int i=0; i<mVehicleTypes.size(); i++)
  {
    if (mVehicleTypes.at(i)->name == typeName)
    {
      index = i;
      break;
    }
  }
  if (index < 0)
  {
    qDebug() << "WORLD: addVehicle> ERROR - could not find VehicleType" << typeName << "for vehicle" << actualName;
    return 0;
  }

  VehicleType *vt = mVehicleTypes.at(index);
  Vehicle *v = new Vehicle(actualName, vt);
  v->setMass(vt->mass);
  v->setWheelBase(vt->wheelBase);
  v->setDragConstant(vt->dragConstant);
  v->setMaxEngineForce(vt->maxEngineForce);
  //v->setMaxSpeed(vt->maxSpeed);
  //v->setMaxTurnRate(vt->maxTurnRate);
  addVehicle(v);
  return v;
}

Vehicle* World::findVehicle(const QString &name)
{
  foreach(Vehicle *v, mVehicles)
  {
    if (v && v->name() == name)
      return v;
  }
  return NULL;
}

void World::addDriver(Driver *d)
{
  mDrivers.push_back(d);
}

Driver* World::addDriver(const QString &name, const QString &typeName)
{
  // Find a name for this driver that doesn't already exist.
  int dCount = mDrivers.size();
  QString origName = name;
  QString actualName = origName;
  if (name.isEmpty())
  {
    origName = "Driver";
    actualName = origName+QString::number(dCount);
  }
  Driver *exists = findDriver(actualName);
  int n = dCount+1;
  while (exists != NULL)
  {
    exists = findDriver(origName+QString::number(n));
    n++;
  }

  // Find driver type
  int index = -1;
  for (int i=0; i<mDriverTypes.size(); i++)
  {
    if (mDriverTypes.at(i)->name == typeName)
    {
      index = i;
      break;
    }
  }

  if (index < 0)
  {
    qDebug() << "WORLD: addDriver> ERROR - could not find DriverType" << typeName << "for driver" << actualName;
    return 0;
  }

  DriverType *dt = mDriverTypes.at(index);
  Driver *d = new Driver(actualName);
  d->mReactionTime = dt->reactionTime;
  d->mPreferredTurnSpeed = dt->preferredTurnSpeed;
  d->mDefaultSpeed = dt->defaultSpeed;
  d->mPreferredAcceleration = dt->preferredAcceleration;
  d->mJerk = dt->jerk;
  addDriver(d);
  return d;
}

Driver* World::findDriver(const QString &name)
{
  foreach(Driver *d, mDrivers)
  {
    if (d && d->name() == name)
      return d;
  }
  return NULL;
}

void World::tick()
{
  foreach(TrafficControl *tc, mTrafficControls) tc->tick();
  foreach(Driver *d, mDrivers) d->tick();
  foreach(Vehicle *v, mVehicles) v->tick();
  mFrameManager.addFrame();
  gSimTime = gSimTime.addMSecs(1000*gSimTimeStep);
  //qDebug() << "WORLD: time=" << gSimTime.toString("hh:mm:ss.zzz");
}

void World::run()
{
  QTime maxTime(0,10); // 10 minutes max
  while (!mExiting && (gSimTime < maxTime))
  {
    //qDebug( "WORLD: RUNNING!" );
    tick();
  }
  qDebug( "WORLD: FINISHED RUNNING!" );
}


